#include "../../roboter/include/dynamica/core.hpp"

template<class T>
void demoPDcontrl(double Pgain, double Dgain, T *robot){

    robot->axes_[0].taucmd = Pgain * (robot->axes_[0].qcmd - robot->axes_[0].qfb);
    robot->axes_[0].taucmd += Dgain * (robot->axes_[0].qdcmd - robot->axes_[0].qdfb);
}